using System;
using Gdk;

namespace simulation
{
	public class CanonicalSystem
	{
		int dim_;
		public double[] q_;
		public double[] qdot_;
		
		public CanonicalSystem (int dim)
		{
			dim_ = dim;
			q_ = new double[dim];
			qdot_ = new double[dim];
		}
		
		public int Dim {
			get { return dim_; }
			set 
			{ 
				dim_ = value; 
				q_ = new double[dim_]; 
				qdot_ = new double[dim_]; 
			}
		}
		
		public void Add (double scale, double[] dq, out CanonicalSystem s) {
			s = new CanonicalSystem(this.Dim);
			for (int i = 0; i < this.Dim; i++)
				s.q_[i] = q_[i] + dq[i] * scale;
		}

		public void Add (double scale, double[] dq) {
			for (int i = 0; i < this.Dim; i++)
				q_[i] += dq[i] * scale;
		}
		
		public void print(string name)
		{
			Console.Write(name + " : q = ");
			for (int i = 0; i < Dim; i++)
				Console.Write(q_[i].ToString() + " ");
			Console.Write(" qdot = ");
			for (int i = 0; i < Dim; i++)
				Console.Write(qdot_[i].ToString() + " ");
			Console.WriteLine();
		}
	}
	
	public interface IMechanicalSystem : ICloneable
	{
		void createCanonical(out CanonicalSystem s);
		void toCanonical(ref CanonicalSystem s);
		void fromCanonical(CanonicalSystem s);
		//void copy(IMechanicalSystem s);
		//int Dim { get; }
	}
	
	public abstract class Force<MechanicalSystem> where MechanicalSystem : IMechanicalSystem
	{
		public abstract void getDot(MechanicalSystem s, ref double[] qdot);
		public void getDot(MechanicalSystem s, ref CanonicalSystem cs)
		{
			getDot(s, ref cs.qdot_);
		}
	}
	
	public class PointMassX : IMechanicalSystem
	{
		public double m_; // the mass
		public double x_; // the x-coordinate
		public double v_; // the velocity in x-direction
		
		public PointMassX (double m, double x, double v) 
		{
			m_ = m;
			x_ = x;
			v_ = v;
		}
		
		public PointMassX ()
		{
		}
		
		public void toCanonical(ref CanonicalSystem s)
		{
			s.q_[0] = x_;
			s.q_[1] = v_;
			//s.qdot_[0] = v_;
			//s.qdot_[1] = 0 / m_;
		}
		
		public void createCanonical(out CanonicalSystem s)
		{
			s = new CanonicalSystem (2);
			s.q_[0] = x_;
			s.q_[1] = v_;
		}
		
		public void fromCanonical(CanonicalSystem s)
		{
			x_ = s.q_[0];
			v_ = s.q_[1];
		}
		
		public int Dim {
			get { return 1; }
		}
		
		public object Clone()
		{
			return new PointMassX(m_,x_,v_);
		} 
		
	}

	public class PointMassXY : IMechanicalSystem
	{
		public double m_; // the mass
		public double x_, y_; // the coordinates
		public double vx_, vy_; // the velocity in xy directions

		public PointMassXY (double m, double x, double y, double vx, double vy) 
		{
			m_ = m;
			x_ = x;
			vx_ = vx;
			y_ = y;
			vy_ = vy;
		}
		
		public PointMassXY ()
		{
		}

		public int Dim {
			get { return 2; }
		}

		public void toCanonical(ref CanonicalSystem s)
		{
			s.q_[0] = x_;
			s.q_[1] = y_;
			s.q_[2] = vx_;
			s.q_[3] = vy_;
			//s.qdot_[0] = v_;
			//s.qdot_[1] = 0 / m_;
		}
		
		public void createCanonical(out CanonicalSystem s)
		{
			s = new CanonicalSystem (4);
			s.q_[0] = x_;
			s.q_[1] = y_;
			s.q_[2] = vx_;
			s.q_[3] = vy_;
		}
		
		public void fromCanonical(CanonicalSystem s)
		{
			x_ = s.q_[0];
			y_ = s.q_[1];
			vx_ = s.q_[2];
			vy_ = s.q_[3];
		}
		
		public object Clone()
		{
			return new PointMassXY(m_,x_,y_,vx_,vy_);
		} 

		public override string ToString ()
		{
			 return "x = " + x_.ToString() + " y = " + y_.ToString() +
				" vx = " + vx_.ToString() + " vy = " + vy_.ToString();
		}
		
		public void draw (Gdk.Window win, double x_unit, double y_unit)
		{
			int width, height;			

			Gdk.GC gc = new Gdk.GC (win);
	
			win.Clear();
			win.GetSize(out width, out height);
			
			gc.Fill = Gdk.Fill.Solid;
			Gdk.Color red_color = new Gdk.Color (0xff, 0, 0);
			//Gdk.Color blue_color = new Gdk.Color (0, 0, 0xff);
			Gdk.Colormap colormap = Gdk.Colormap.System;
			colormap.AllocColor(ref red_color, true, true);
			//colormap.AllocColor(ref blue_color, true, true);
	
			gc.Foreground = red_color;
			win.DrawArc(gc, true, width / 2 + (int)(x_ * x_unit), height / 2 - (int) (y_ * y_unit), 5, 5, 0, 360*64); // full circle
	
			//gc.Foreground = blue_color;
			//win.DrawArc(gc, true, width / 2 + (int)(m_state.x2 * x_unit), height / 2 - (int) (m_state.y2 * y_unit), 5, 5, 0, 360*64); // full circle
		}
	}
	
	public class ODESolver<MechanicalSystem> where MechanicalSystem : IMechanicalSystem
	{
		public double step_;
		public Force<MechanicalSystem> force_;
		
		public ODESolver(double step, Force<MechanicalSystem> force) 
		{
			step_ = step;
			force_ = force;
		}

		public virtual void solve(MechanicalSystem s_prev, out MechanicalSystem s_next)
		{
			s_next = s_prev;
		}
		
		public double Step
		{
			get { return step_; }
			set { step_ = value; }
		}
	}
	
	public class EulerSolver<MechanicalSystem> : ODESolver<MechanicalSystem> 
		where MechanicalSystem : IMechanicalSystem, new()
	{
		public EulerSolver(double step, Force<MechanicalSystem> force) : base(step, force)
		{
		}

		public override void solve(MechanicalSystem s_prev, out MechanicalSystem s_next)
		{
			CanonicalSystem s;
			s_prev.createCanonical(out s);
			force_.getDot(s_prev, ref s);

			s.Add(step_, s.qdot_);
			s_next = new MechanicalSystem ();
			s_next.fromCanonical(s);
		}
	}

	public class RK4Solver<MechanicalSystem> : ODESolver<MechanicalSystem> 
		where MechanicalSystem : IMechanicalSystem, new()
	{
		public RK4Solver(double step, Force<MechanicalSystem> force) : base(step, force)
		{
		}

		public override void solve(MechanicalSystem s1, out MechanicalSystem s_next)
		{
			CanonicalSystem y1, y2, y3, y4;

			s1.createCanonical(out y1);	force_.getDot(s1, ref y1); //y1.print("y1");	
			y1.Add(0.5*step_, y1.qdot_, out y2);
			
			s1.fromCanonical(y2); force_.getDot(s1, ref y2); ///y2.print("y2");	y1.print("y1");	
			y1.Add(0.5*step_, y2.qdot_, out y3);
			
			s1.fromCanonical(y3); force_.getDot(s1, ref y3); //y3.print("y3");	y1.print("y1");	
			y1.Add(step_, y3.qdot_, out y4);
			
			s1.fromCanonical(y4); force_.getDot(s1, ref y4); //y4.print("y4");	y1.print("y1");	

			CanonicalSystem y_next;
			y1.Add(1.0/6*step_, y1.qdot_, out y_next); //y_next.print("y_next");
			y_next.Add(1.0/6*2*step_, y2.qdot_); //y_next.print("y_next");
			y_next.Add(1.0/6*2*step_, y3.qdot_); //y_next.print("y_next");
			y_next.Add(1.0/6*step_, y4.qdot_); //y_next.print("y_next");

			s_next = (MechanicalSystem) s1.Clone();
			s_next.fromCanonical(y_next);
		}
	}
}

